#pragma once

#include <unordered_map>
#include <map>
#include <memory>

#include "backendshen/edgeshen.h"
#include "backendshen/vertexshen.h"
#include "backendshen/eigen_types.h"

using namespace std;
namespace myslam{
namespace backend{
class Problem
{
public:
    enum class ProblemType
    {
        SLAM_PROBLEM,
        GENERIC_PROBLEM
    };
/*
1. HashVertex = std::map<ulong, std::shared_ptr<Vertex>>
底层实现：基于红黑树（有序关联容器）。
键类型：ulong（顶点ID）。
值类型：std::shared_ptr<Vertex>（顶点智能指针）。
特点：
按键自动排序（从小到大）。
查找、插入、删除的时间复杂度为 O(log n)。
适用于需要按顶点ID 有序遍历 的场景。
典型用途：
存储所有顶点，并按ID顺序访问（如调试、可视化或需要顺序处理的场景）。

2. HashEdge = std::unordered_map<ulong, std::shared_ptr<Edge>>
底层实现：基于哈希表（无序关联容器）。
键类型：ulong（边ID）。
值类型：std::shared_ptr<Edge>（边智能指针）。
特点：
无序存储，但查找效率更高。
平均时间复杂度为 O(1)（最坏情况 O(n)）。
比 std::map 更节省内存（无红黑树开销）。
典型用途：
快速通过边ID查找边（如优化过程中的边管理）。

3. HashVertexIdToEdge = std::unordered_multimap<ulong, std::shared_ptr<Edge>>
底层实现：基于哈希表（无序关联容器）。
键类型：ulong（顶点ID）。
值类型：std::shared_ptr<Edge>（边智能指针）。
特点：
允许多个边关联同一个顶点ID（即一个顶点可能对应多条边）。
无序存储，查找效率平均 O(1)（最坏 O(n)）。
典型用途：
建立顶点到边的 一对多关系（如查询某个顶点关联的所有边）。

*/
    using ulong = unsigned long;
    //这里顶点使用std::map存储  因为顶点保存都是有序的  避免哈希冲突，在多线程中，有序的处理顶点。   顶点相比于边的数量较少，
    using HashVertex = std::map<ulong, std::shared_ptr<Vertex>>;
    //边是无序的，只需要快速查找即可
    using HashEdge = std::unordered_map<ulong, std::shared_ptr<Edge>>;
    using HashVertexIdToEdge = std::unordered_multimap<ulong, std::shared_ptr<Edge>>;
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    Problem(ProblemType problemType);
    ~Problem();
    bool AddVertex(std::shared_ptr<Vertex> vertex);
    bool AddEdge(shared_ptr<Edge> edge);
    bool Solve(int iterations);

private:
    void SetOrdering();
    void MakeHessian();
    void ComputeLambdaInitLM();
    void AddLambdatoHessianLM();
    void SolveLinearSystem();
    void RemoveLambdaHessianLM();
    void UpdateStates();
    bool IsGoodStepInLM();
    void RollbackStates();  
    double currentLambda_;
    double currentChi_;
    double stopThresholdLM_;  //LM迭代退出阈值的条件
    double ni_;               //控制Lambda缩放大小
    ProblemType problemType_;
    ///整个信息矩阵
    MatXX Hessian_;
    VecX b_;
    VecX delta_x_;


    ///先验部分信息
    MatXX H_prior_;
    VecX b_prior_;
    MatXX Jt_prior_inv_;
    VecX err_prior_;

    //Heesian 的Landmark 和pose 部分
    MatXX H_pp_;
    VecX b_pp_;
    MatXX H_ll_;
    VecX b_ll_;

    //all vertices
    HashVertex verticies_;
    //all edge 
    HashEdge edges_;
    //有vertex 的id 查询edge
    HashVertexIdToEdge vertexToEdge_;

    //Ordering related
    ulong ordering_poses_=0;
    ulong ordering_landmarks_=0;
    ulong ordering_generic_=0;


    //verticies need to marg 
    HashVertex verticies_marg_;

    bool bDebug=false;
    double t_hessian_cost_=0.0;
    double t_PCGsolve_cost_ = 0.0;
};
}
} // namespace myslam
